# -*- encoding: utf-8 -*-
"""
@File    :   walkToObstacle.py    

@Contact :   2055466817@qq.com

@Modify Time :   2020/12/2 上午8:05 

@Author :   赵方国        
"""
import almath
from naoqi import ALProxy
import cv2 as cv
import random
from cmath import pi
import time
from ImagProcess import getImag
from ImagProcess import ImagProgressHSV
from obstacle import correctAngle
from correct_X_Y import detect_angleXY_to_keep_stable


# def walkToObstacle(robotIP, PORT, angleINIT):
#     smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
#     lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动
#
#     motionProxy = ALProxy("ALMotion", robotIP, PORT)
#     postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
#     valueProxy = ALProxy("ALMemory", robotIP, PORT)
#     tts = ALProxy("ALTextToSpeech", robotIP, PORT)
#
#     angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
#     print angle
#     print 'angle: ' + str(angle)
#     print 'trurned: ' + str(angle - angleINIT)
#     while abs(angle - angleINIT) > 5 * pi / 180:
#         motionProxy.moveTo(0, 0, angle - angleINIT)
#         print 'turn'
#     width = 10000
#     height = 10000
#     name = str(random.randint(0, 1000))
#     data = getImag(robotIP, 9559, 0, "blue", name)  # 使用上摄像头
#     # while width * height <= 18000:  # TODO 此处为粗调阈值，面积大于18000
#     #     name = str(random.randint(0, 1000))
#     #     data = getImag(robotIP, 9559, 0, name)
#     #     width = data[2]
#     #     height = data[3]
#     #     print width * height
#     #     motionProxy.moveTo(0.2, 0, 0, smallTurnStep)  # TODO 此处的连续行走参数需要调整
#     motionProxy.moveTo(0.2, 0, 0, smallTurnStep)
#     correctAngle(robotIP, PORT, angleINIT)
#     name = str(random.randint(0, 1000))
#     data = getImag(robotIP, 9559, 1, "blue", name)  # 使用下摄像头
#     y = data[1]
#     h = data[2]
#     w = data[3]
#     angle_x = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value")
#     angle_y = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value")
#     while h <= 145 or w <= 170:  # TODO 此处为细调阈值 y+h > 120
#         detect_angleXY_to_keep_stable(robotIP, 9559, angle_x, angle_y)
#         name = str(random.randint(0, 1000))
#         data = getImag(robotIP, 9559, 1, "blue", name)
#         y = data[1]
#         h = data[2]
#         w = data[3]
#         print data
#         motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
#         correctAngle(robotIP, PORT, angleINIT)
#
#     tts.say('finish')
#     time.sleep(3)
#

def walkToObstacleTOWARDversion(robotIP, PORT, angleINIT, flag):
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    correctAngle(robotIP, PORT, angleINIT)
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    name = str(random.randint(0, 1000))
    data = getImag(robotIP, 9559, 1, "blue", name)  # 使用下摄像头
    h = data[2]
    w = data[3]
    angle_x = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value")
    angle_y = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value")
    while h <= 140 or w <= 175:  # TODO 此处为细调阈值 y+h > 120
        # motionProxy.moveTo(0.1, 0, 0, smallTurnStep)
        detect_angleXY_to_keep_stable(robotIP, 9559, angle_x, angle_y)
        motionProxy.moveToward(0.4, 0, 0, lower_steps)
        name = str(random.randint(0, 1000))
        data = getImag(robotIP, 9559, 1, "blue", name)
        # y = data[1]
        x = data[0]
        h = data[2]
        w = data[3]
        center = x + h / 2
        print h, center
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove = robotMove[0]
        if robotMove > 0.2:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    tts.say('finish')
    motionProxy.stopMove()
    correctAngle(robotIP, PORT, angleINIT)
    time.sleep(1)


# def alignToObstacle(robotIP, PORT):
#     """
#     对齐障碍物，进行左右旋转
#
#     :param robotIP:
#     :param PORT:
#     :return: 返回的是angleZ的值，应作为angleINIT使用
#     """
#     motionProxy = ALProxy("ALMotion", robotIP, PORT)
#     postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
#     valueProxy = ALProxy("ALMemory", robotIP, PORT)
#     tts = ALProxy("ALTextToSpeech", robotIP, PORT)
#     name = str(random.randint(0, 1000))
#     data = getImag(robotIP, PORT, 0, "blue", name)
#     center = data[0] + data[2] / 2
#     print center
#     if 140 > center > 100:  # TODO 此处的范围有问题，需要更改
#         time.sleep(1)
#         angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
#         return angle
#     if center < 100:
#         motionProxy.moveTo(0, 0, pi / 12)
#         alignToObstacle(robotIP, PORT)
#     if center > 140:
#         motionProxy.moveTo(0, 0, -pi / 12)
#         alignToObstacle(robotIP, PORT)


def walkToEnd(robotIP, PORT, angleINIT):
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    name = random.randint(0, 1000)  # 没有物体时会输出【0， 0， 0， 0】
    data = [100, 100, 100, 100]
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    while True:
        name = str(random.randint(0, 1000))
        data = getImag(robotIP, PORT, 1, "yellow", name)
        motionProxy.moveToward(0.4, 0, 0, lower_steps)
        if (data[0] + data[1] + data[2] + data[3]) == 0 or (data[2] + data[3] < 10):
            break
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove = robotMove[0]
        if robotMove > 0.2:
            correctAngle(robotIP, PORT, angleINIT)
            initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    motionProxy.moveTo(0.5, 0, 0, smallTurnStep)
    return True


if __name__ == '__main__':
    robotIP = "169.254.148.118"
    PORT = 9559
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)

    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    angleINTI = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # angleIMIT = alignToObstacle(robotIP, PORT)
    # print "angle INIT"
    # print angleIMIT
    # walkToObstacle(robotIP, PORT, angleIMIT)
    walkToEnd(robotIP, PORT, angleINTI)
    motionProxy.rest()
    cv.waitKey(0)
